// main.c

// standard libs
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>

#include "main.h"
#include "../utils/LCD_functions.h"
#include "../utils/LCD_driver.h"
#include "../utils/button.h"
#include "../utils/util.h"

extern volatile char KEY_VALID;


// Motor control circuit is connected to PortB  
//                              +---+---+---+---+---+
//            Port B            | 1 | 3 | x | x | V |  
//     Motor Contorl pins       +---+---+---+---+---+
//                              | 0 | 2 | x | x | G |  
//                              +---+---+---+---+---+
//  PB0 - Left motor PWM
//  PB1 - Right motor PWM
//  PB2 - Left motor Dir
//  PB3 - Right motor Dir
//
//
// Sensors are connected to ADC4(PF4) thru ADC7(PF7).  Port F is also known as the JTAG port and 
// JTAG must be disabled to use these pins.  The pins are not numbered the same as the other connectors.
//                              +---+---+---+---+---+
//            Port F  (JTAG)    | x | x | x | x | x |  Pins marked with a "x" should not be used.  
//            Usable Pins       +---+---+---+---+---+
//                              | 7 | x | 5 | 6 | 4 |  
//                              +---+---+---+---+---+

#define rightLineSensor	4		//PF4 - Bottom Right pin
#define leftLineSensor	5		//PF5 - 2 to the left of PF4	
#define RightDistSensor	6		//PF6 - 1 to the left of PF4 	
#define LeftDistSensor	7		//PF7 - Bottom Left pin 	

#define msLoop			5		// to control loop speed
#define msWaitStart		3000	// number of milliseconds to wait before start
#define sumOffLine		100		// off the line if sensor sum is below this
#define pwmDrive		9		// PWM speed for driving

#define DistThreshold	112		// Threshold for Distance sensor


int main (void) { 
    Initialization();

    show_mhz();				// Display CPU speed
    TimerWait(1000);		// Wait 1 second

    while(1) {
        printLCD("Show ADCs");				// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)	{				// If enter was pressed then do what is in the braces, just skip over it. 
		    TimerWait(500);					// debounce joystick button
            showADC();						                   
            printLCD("Back ADCs");
            TimerWait(2000);
		}
  
        printLCD("Servo");					// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)	{				// If enter was pressed then do what is in the braces, else just continue.
		    TimerWait(500);					// debounce joystick button
            servo_test();                   
            printLCD("Back Servo");
            TimerWait(2000);
		}
  
		printLCD("Fast Loop");				// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1)	{				// If enter was pressed then do what is in the braces, else just continue.
            TimerWait(500);					// debounce joystick button
            FastLoop();
            printLCD("Back Fast Loop");
            TimerWait(3000);
        }

        printLCD("Line Follow");			// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1) {				// If enter was pressed then do what is in the braces, else just continue.
            TimerWait(500);					// debounce joystick button
            LineFollow();
            printLCD("Back Line Follow");
            TimerWait(3000);
        }
  
        printLCD("PWM Test");			// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1) {			// If enter was pressed then do what is in the braces, else just continue.
            TimerWait(500);				// debounce joystick button
            PWM_Test();
            printLCD("Back PWM Test");
            TimerWait(3000);
        }
  
        printLCD("USART Test");			// Display the text in quotes on the LCD
        while (!KEY_VALID);				// Wait for joystick to be moved or pressed.
        if (getkey() == 1) {			// If enter was pressed then do what is in the braces, else just continue.
            TimerWait(500);				// debounce joystick button
            usart_test();
            printLCD("Back USART Test");
            TimerWait(3000);
        }
    }   
}  


void showADC(void) {
    unsigned char adcL, adcR = 0;
   
    SetupLineSensors();
    InitADC();
   
    while (!(getkey() == 1)) {			//do the following block until enter is pressed
        adcL = GetADC(leftLineSensor);
        adcR = GetADC(rightLineSensor);
		LCD_Colon(1);
        show3hex(adcL, adcR, adcR);
        TimerWait(500);
    }
}


//*****************************************************************************************************
//*  Fast loop - should be started with one sensor on the black line and the other sensor off the line.
//*****************************************************************************************************  
void FastLoop(void) 
{
    unsigned char adcL, adcR;
	unsigned char Threshold;
   
    disable_JTAG();				// Disable JTAG port, making Port F available for line sensors and such.
    SetupLineSensors();			// Setup the line sensors
    InitADC();					// Initialize the ADCs for the sensors
   
    // read line sensor values
    adcL = GetADC(leftLineSensor);
    adcR = GetADC(rightLineSensor);

    Threshold = (adcL + adcR) / 2;
  
    while (!(getkey() == 1))	//do until enter is pressed
    {
        // read line sensor values
        adcL = GetADC(leftLineSensor);
        adcR = GetADC(rightLineSensor);

		//if (GetADC(RightDistSensor) < DistThreshold)
		//{

			if (adcL < Threshold)  
				SetLeftMotorPWM(10);
			else
				SetLeftMotorPWM(3);

			if (adcR < Threshold)
				SetRightMotorPWM(10);
			else
				SetRightMotorPWM(3);
		/*}
		//else
		{
			SetLeftMotorPWM(0);
			SetRightMotorPWM(0);
		} */
    }
    // Done
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}


//*****************************************************************************************************
//*  LineFollow - 
//*****************************************************************************************************  
void LineFollow(void) {
    unsigned char adcL, adcR;
    int sensorDifference;
   
    SetupLineSensors();				// Setup the line sensors
    InitADC();						// Initialize the ADCs for the sensors
   
    while (!(getkey() == 1)) {      //do the folling block until enter is pressed
		/* read line sensor values */
		adcL = GetADC(leftLineSensor);
		adcR = GetADC(rightLineSensor);

		/* calculate sum and difference */
		sensorDifference = (adcL - adcR) / 16;

		//if (GetADC(RightDistSensor) < DistThreshold)
		//{
			/* steer in proportion to the difference between the sensors */
			SetLeftMotorPWM(pwmDrive - sensorDifference);
			SetRightMotorPWM(pwmDrive + sensorDifference);
		//}
		//else
		//{
		//	SetLeftMotorPWM(0);
		//	SetRightMotorPWM(0);
		//}
	}
    // Done
	SetLeftMotorPWM(0);
    SetRightMotorPWM(0);
}


//******************************************** 
//* PWM test - check wiring and such
//******************************************** 
void PWM_Test(void) {
    while (!(getkey() == 1)) {
 
	    LCD_puts_f(PSTR("LFWD"),1);
	    SetLeftMotorPWM(10);
        SetRightMotorPWM(0);
        TimerWait(3000);
	 
        LCD_puts_f(PSTR("LREV"),1);
	    SetLeftMotorPWM(-10);
        SetRightMotorPWM(0);
        TimerWait(3000);
    
        LCD_puts_f(PSTR("RFWD"),1);
	    SetLeftMotorPWM(0);
        SetRightMotorPWM(10);
        TimerWait(3000);
	
        LCD_puts_f(PSTR("RREV"),1);
	    SetLeftMotorPWM(0);
        SetRightMotorPWM(-10);
        TimerWait(3000);
	
        LCD_puts_f(PSTR("2 FWD low"),1);
		SetRightMotorPWM(5);
	    SetLeftMotorPWM(5);
	    TimerWait(3000);

        LCD_puts_f(PSTR("2 REV med"),1);
	    SetRightMotorPWM(-8);
	    SetLeftMotorPWM(-8);
	    TimerWait(3000);
    
        LCD_puts_f(PSTR("CCW"),1);
		SetRightMotorPWM(8);
	    SetLeftMotorPWM(-8);
	    TimerWait(3000);

        LCD_puts_f(PSTR("CW"),1);
	    SetRightMotorPWM(-8);
	    SetLeftMotorPWM(8);
	    TimerWait(3000);
	}
    // Done
	SetLeftMotorPWM(0);
	SetRightMotorPWM(0);
}
 
//******************************************** 
//* Test Servo 
//******************************************** 
void servo_test(void) { 

	init_servo();			// Initialize Servo

	servo_center();			// center servo (send 1.5ms pulses)
	TimerWait(5000);		// Wait 5 seconds (5000 ms)

    while (!(getkey() == 1)) { 
         servo_up();		// turn 45deg CCW from center 
		 TimerWait(2000);	// Wait 2 seconds (2000 ms)

         servo_down();		// turn 45deg CW from center 
		 TimerWait(2000);	// Wait 2 seconds (2000 ms)
    }  
}


//******************************************** 
//* USART Test  
//******************************************** 
void usart_test(void) { 

//	USART_Init(103);		// Initialize USART 9.6Kb
	USART_Init(51);			// Initialize USART 19.2Kb
//	USART_Init(25);			// Initialize USART 38.4Kb
//	USART_Init(3);			// Initialize USART 57.6Kb (2.1% error)

    while (!(getkey() == 1)) {
		sendString("Hello World\r\n");
		TimerWait(100);		// Wait 1 seconds (100 ms)
    }  
}
